﻿using System;
using System.Collections.Generic;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Shapes;
using System.Drawing;

using Microsoft.Kinect;

namespace K4W.BasicOverview.UI
{
    public partial class MainWindow : Window
    {
        /// <summary>
        /// Size fo the RGB pixel in bitmap
        /// </summary>
        private readonly int _bytePerPixel = (PixelFormats.Bgr32.BitsPerPixel + 7) / 8;

        /// <summary>
        /// Representation of the Kinect Sensor
        /// </summary>
        private KinectSensor _kinect = null;

        /// <summary>
        /// FrameReader for our coloroutput
        /// </summary>
        private ColorFrameReader _colorReader = null;

        /// <summary>
        /// FrameReader for our depth output
        /// </summary>
        private DepthFrameReader _depthReader = null;

        /// <summary>
        /// FrameReader for our infrared output
        /// </summary>
        private InfraredFrameReader _infraReader = null;

        /// <summary>
        /// FrameReader for our body output
        /// </summary>
        private BodyFrameReader _bodyReader = null;

        /// <summary>
        /// Array of color pixels
        /// </summary>
        private byte[] _colorPixels = null;

        /// <summary>
        /// Array of depth pixels used for the output
        /// </summary>
        private byte[] _depthPixels = null;

        /// <summary>
        /// Array of infrared pixels used for the output
        /// </summary>
        private byte[] _infraPixels = null;

        /// <summary>
        /// Array of depth values
        /// </summary>
        private ushort[] _depthData = null;

        /// <summary>
        /// Array of infrared data
        /// </summary>
        private ushort[] _infraData = null;

        /// <summary>
        /// All tracked bodies
        /// </summary>
        private Body[] _bodies = null;

        private Point[] _points = null;

        private Point currentPoint = new Point();
        private Point nextPoint = new Point();

        /// <summary>
        /// Color WriteableBitmap linked to our UI
        /// </summary>
        private WriteableBitmap _colorBitmap = null;

        /// <summary>
        /// Color WriteableBitmap linked to our UI
        /// </summary>
        private WriteableBitmap _depthBitmap = null;

        /// <summary>
        /// Infrared WriteableBitmap linked to our UI
        /// </summary>
        private WriteableBitmap _infraBitmap = null;


        /// <summary>
        /// Default CTOR
        /// </summary>
        public MainWindow()
        {
            //构建用户界面
            InitializeComponent(); 

            // Initialize Kinect
            InitializeKinect();

            // Close Kinect when closing app
            Closing += OnClosing;
        }

        /// <summary>
        /// Close Kinect & Kinect Service
        /// </summary>
        private void OnClosing(object sender, System.ComponentModel.CancelEventArgs e)
        {
            // Close Kinect
            if (_kinect != null) _kinect.Close();
        }


        #region INITIALISATION
        /// <summary>
        /// Initialize Kinect Sensor
        /// </summary>
        private void InitializeKinect()
        {
            // Get first Kinect
            _kinect = KinectSensor.GetDefault();

            if (_kinect == null) return;

            // Open connection
            _kinect.Open();

            // Initialize Camera
            InitializeCamera();

            // Initialize Depth
            InitializeDepth();

            // Initialize Infrared
            InitializeInfrared();

            // Initialize Body
            IntializeBody();
        }

        /// <summary>
        /// Initialize Kinect Camera
        /// </summary>
        private void InitializeCamera()
        {
            if (_kinect == null) return;

            // Get frame description for the color output
            FrameDescription desc = _kinect.ColorFrameSource.FrameDescription;

            // Get the framereader for Color
            _colorReader = _kinect.ColorFrameSource.OpenReader();

            // Allocate pixel array
            _colorPixels = new byte[desc.Width * desc.Height * _bytePerPixel];

            // Create new WriteableBitmap
            _colorBitmap = new WriteableBitmap(desc.Width, desc.Height, 96, 96, PixelFormats.Bgr32, null);

            // Link WBMP to UI
            CameraImage.Source = _colorBitmap;

            // Hook-up event, 应该是指不停地读入图像数据。
            _colorReader.FrameArrived += OnColorFrameArrived;
        }

        /// <summary>
        /// Initialize Kinect Depth
        /// </summary>
        private void InitializeDepth()
        {
            if (_kinect == null) return;

            // Get frame description for the color output
            FrameDescription desc = _kinect.DepthFrameSource.FrameDescription;

            // Get the framereader for Color
            _depthReader = _kinect.DepthFrameSource.OpenReader();

            // Allocate pixel array
            _depthData = new ushort[desc.Width * desc.Height];
            _depthPixels = new byte[desc.Width * desc.Height * _bytePerPixel];

            // Create new WriteableBitmap
            _depthBitmap = new WriteableBitmap(desc.Width, desc.Height, 96, 96, PixelFormats.Bgr32, null);

            // Link WBMP to UI
            DepthImage.Source = _depthBitmap;

            // Hook-up event
            _depthReader.FrameArrived += OnDepthFrameArrived;
        }

        /// <summary>
        /// Initialize Kinect Infrared
        /// </summary>
        private void InitializeInfrared()
        {
            if (_kinect == null) return;

            // Get frame description for the color output
            FrameDescription desc = _kinect.InfraredFrameSource.FrameDescription;

            // Get the framereader for Color
            _infraReader = _kinect.InfraredFrameSource.OpenReader();

            // Allocate pixel array
            _infraData = new ushort[desc.Width * desc.Height];
            _infraPixels = new byte[desc.Width * desc.Height * _bytePerPixel];

            // Create new WriteableBitmap
            _infraBitmap = new WriteableBitmap(desc.Width, desc.Height, 96, 96, PixelFormats.Bgr32, null);

            // Link WBMP to UI
            InfraredImage.Source = _infraBitmap;

            // Hook-up event
            _infraReader.FrameArrived += OnInfraredFrameArrived;
        }

        /// <summary>
        /// Initialize Body Tracking
        /// </summary>
        private void IntializeBody()
        {
            if (_kinect == null) return;

            // Allocate Bodies array
            _bodies = new Body[_kinect.BodyFrameSource.BodyCount];

            // Open reader
            _bodyReader = _kinect.BodyFrameSource.OpenReader();

            // Hook-up event
            _bodyReader.FrameArrived += OnBodyFrameArrived;
        }
        #endregion INITIALISATION


        #region FRAME PROCESSING
        /// <summary>
        /// Process color frames & show in UI
        /// </summary>
        private void OnColorFrameArrived(object sender, ColorFrameArrivedEventArgs e)
        {
            // Get the reference to the color frame
            ColorFrameReference colorRef = e.FrameReference;

            if (colorRef == null) return;

            // Acquire frame for specific reference
            ColorFrame frame = colorRef.AcquireFrame();

            // It's possible that we skipped a frame or it is already gone
            if (frame == null) return;

            using (frame)
            {
                // Get frame description
                FrameDescription frameDesc = frame.FrameDescription;

                // Check if width/height matches
                if (frameDesc.Width == _colorBitmap.PixelWidth && frameDesc.Height == _colorBitmap.PixelHeight)
                {
                    // Copy data to array based on image format
                    if (frame.RawColorImageFormat == ColorImageFormat.Bgra)
                    {
                        frame.CopyRawFrameDataToArray(_colorPixels);
                    }
                    else frame.CopyConvertedFrameDataToArray(_colorPixels, ColorImageFormat.Bgra);

                    // Copy output to bitmap
                    _colorBitmap.WritePixels(
                            new Int32Rect(0, 0, frameDesc.Width, frameDesc.Height),
                            _colorPixels,
                            frameDesc.Width * _bytePerPixel,
                            0);
                }
            }
        }

        /// <summary>
        /// Process the depth frames and update UI
        /// </summary>
        private void OnDepthFrameArrived(object sender, DepthFrameArrivedEventArgs e)
        {
            DepthFrameReference refer = e.FrameReference;

            if (refer == null) return;

            DepthFrame frame = refer.AcquireFrame();

            if (frame == null) return;

            using (frame)
            {
                FrameDescription frameDesc = frame.FrameDescription;

                if (((frameDesc.Width * frameDesc.Height) == _depthData.Length) && (frameDesc.Width == _depthBitmap.PixelWidth) && (frameDesc.Height == _depthBitmap.PixelHeight))
                {
                    // Copy depth frames
                    frame.CopyFrameDataToArray(_depthData);

                    // Get min & max depth
                    ushort minDepth = frame.DepthMinReliableDistance;
                    ushort maxDepth = frame.DepthMaxReliableDistance;

                    // Adjust visualisation
                    int colorPixelIndex = 0;
                    for (int i = 0; i < _depthData.Length; ++i)
                    {
                        // Get depth value
                        ushort depth = _depthData[i]; //这个很重要啊，这个depthData应该就是深度数据。
                        if (_depthData !=null && _depthData.Length >0)
                        {
                            //PixelDepth.Text = string.Format("{0}mm, depth"); //并不是这样就可以啊。
                        }
                        
                        if (depth == 0)
                        {
                            _depthPixels[colorPixelIndex++] = 41;
                            _depthPixels[colorPixelIndex++] = 239;
                            _depthPixels[colorPixelIndex++] = 242;
                        }
                        else if (depth < minDepth || depth > maxDepth)
                        {
                            _depthPixels[colorPixelIndex++] = 25;
                            _depthPixels[colorPixelIndex++] = 0;
                            _depthPixels[colorPixelIndex++] = 255;
                        }
                        else
                        {
                            double gray = (Math.Floor((double)depth / 250) * 12.75);

                            _depthPixels[colorPixelIndex++] = (byte)gray;
                            _depthPixels[colorPixelIndex++] = (byte)gray;
                            _depthPixels[colorPixelIndex++] = (byte)gray;
                        }

                        // Increment
                        ++colorPixelIndex;
                    }

                    // Copy output to bitmap
                    _depthBitmap.WritePixels(
                            new Int32Rect(0, 0, frameDesc.Width, frameDesc.Height),
                            _depthPixels,
                            frameDesc.Width * _bytePerPixel,
                            0);
                }
            }
        }

        /// <summary>
        /// Process the infrared frames and update UI
        /// </summary>
        private void OnInfraredFrameArrived(object sender, InfraredFrameArrivedEventArgs e)
        {
            // Reference to infrared frame
            InfraredFrameReference refer = e.FrameReference;

            if (refer == null) return;

            // Get infrared frame
            InfraredFrame frame = refer.AcquireFrame();

            if (frame == null) return;

            // Process it
            using (frame)
            {
                // Get the description
                FrameDescription frameDesc = frame.FrameDescription;

                if (((frameDesc.Width * frameDesc.Height) == _infraData.Length) && (frameDesc.Width == _infraBitmap.PixelWidth) && (frameDesc.Height == _infraBitmap.PixelHeight))
                {
                    // Copy data
                    frame.CopyFrameDataToArray(_infraData);

                    int colorPixelIndex = 0;

                    for (int i = 0; i < _infraData.Length; ++i)
                    {
                        // Get infrared value
                        ushort ir = _infraData[i];

                        // Bitshift
                        byte intensity = (byte)(ir >> 8);

                        // Assign infrared intensity
                        _infraPixels[colorPixelIndex++] = intensity;
                        _infraPixels[colorPixelIndex++] = intensity;
                        _infraPixels[colorPixelIndex++] = intensity;

                        ++colorPixelIndex;
                    }

                    // Copy output to bitmap
                    _infraBitmap.WritePixels(
                            new Int32Rect(0, 0, frameDesc.Width, frameDesc.Height),
                            _infraPixels,
                            frameDesc.Width * _bytePerPixel,
                            0);
                }
            }
        }

        /// <summary>
        /// Process the body-frames and draw joints
        /// </summary>
        private void OnBodyFrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            
            // Get frame reference
            BodyFrameReference refer = e.FrameReference;

            if (refer == null) return;

            // Get body frame
            BodyFrame frame = refer.AcquireFrame();

            if (frame == null) return;

            currentPoint = nextPoint;

            using (frame) //using可以自动释放资源
            {
                // Aquire body data
                frame.GetAndRefreshBodyData(_bodies);
                
                // Clear Skeleton Canvas
                SkeletonCanvas.Children.Clear();

                // Loop all bodies，这里最好改成只识别一个人
                foreach (Body body in _bodies)
                {

                    // Only process tracked bodies
                    if (body.IsTracked)
                    {
                        //DrawBody(body);
                        //Point currentPoint = GetJointPoint(body.Joints[JointType.HandRight]);
                        nextPoint = GetJointPoint(body.Joints[JointType.HandRight]);
                        Paint(currentPoint, nextPoint, SkeletonCanvas);
                    }                 
                }

            }
        }

        /// <summary>
        /// Visualize the body
        /// </summary>
        /// <param name="body">Tracked body</param>
        private void DrawBody(Body body)
        {
            // Draw points
            Polyline trajec = new Polyline();
            trajec.StrokeThickness = 8;
            trajec.Stroke = Brushes.Black;
            foreach (JointType type in body.Joints.Keys)
            {
                ColorSpacePoint point = _kinect.CoordinateMapper.MapCameraPointToColorSpace(body.Joints[type].Position);
                trajec.Points.Add(GetJointPoint(body.Joints[type]));
                if (float.IsInfinity(point.X) || float.IsInfinity(point.X)) return;
                Canvas.SetLeft(trajec, point.X / 2);
                Canvas.SetTop(trajec, point.Y / 2);
            }
            
            SkeletonCanvas.Children.Add(trajec);

        }
        private Point GetJointPoint(Joint joint)
        {
            ColorSpacePoint point = _kinect.CoordinateMapper.MapCameraPointToColorSpace(joint.Position);
            return new Point(point.X, point.Y); //不是很懂，怎么对齐画布

        }
        public void Paint(Point currentPoint, Point nextPoint, Canvas SkeletonCanvas)
        {
            Line line = new Line();

            line.Stroke = Brushes.Black;

            line.StrokeThickness = 10;

            line.X1 = currentPoint.X;
            line.Y1 = currentPoint.Y;
            line.X2 = nextPoint.X;
            line.Y2 = nextPoint.Y;

            //Canvas.SetLeft(line, currentPoint.X / 2);
            //Canvas.SetTop(line, currentPoint.Y / 2);

            SkeletonCanvas.Children.Add(line);


        }
        #endregion FRAME PROCESSING

        #region UI Methods
        private void OnToggleCamera(object sender, RoutedEventArgs e)
        {
            ChangeVisualMode("Camera");
        }

        private void OnToggleDepth(object sender, RoutedEventArgs e)
        {
            ChangeVisualMode("Depth");
        }

        private void OnToggleInfrared(object sender, RoutedEventArgs e)
        {
            ChangeVisualMode("Infrared");
        }

        /// <summary>
        /// Change the UI based on the mode
        /// </summary>
        /// <param name="mode">New UI mode</param>
        private void ChangeVisualMode(string mode)
        {
            // Invis all
            CameraGrid.Visibility = Visibility.Collapsed;
            DepthGrid.Visibility = Visibility.Collapsed;
            InfraredGrid.Visibility = Visibility.Collapsed;

            switch (mode)
            {
                case "Camera":
                    CameraGrid.Visibility = Visibility.Visible;
                    break;
                case "Depth":
                    DepthGrid.Visibility = Visibility.Visible;
                    break;
                case "Infrared":
                    InfraredGrid.Visibility = Visibility.Visible;
                    break;
            }
        }
        #endregion UI Methods
    }
}

